import launch
import launch_ros
from ament_index_python.packages import get_package_share_directory
import os
def generate_launch_description():
    robot_name_in_model = "fishbot"
    #获取默认的urdf路径--通过CMakeLists.txt安装
    urdf_package_path = get_package_share_directory('fishbot_description')
    default_xacro_path = os.path.join(urdf_package_path, 'urdf', 'fishbot','fishbot.urdf.xacro')
    #default_rviz_config_path = os.path.join(urdf_package_path, 'config', 'display_robot_model.rviz')
    default_gazebo_world_path = os.path.join(urdf_package_path, 'world', 'custom_room.world')
    #声明一个urdf目录参数，方便修改，launch参数
    action_declare_arg_mode_path = launch.actions.DeclareLaunchArgument(
        name='model', default_value= str(default_xacro_path), description='加载的模型文件路径'
    )

    #通过文件路径，获取内容用cat命令，并转换成参数值对象，以供传入robot_state_publisher
    substitutions_commond_result = launch.substitutions.Command(['xacro ', launch.substitutions.LaunchConfiguration('model')])
    robot_description_value= launch_ros.parameter_descriptions.ParameterValue(substitutions_commond_result, value_type=str)
    
    #启动节点,状态发布节点--发布/robot_description话题 
    action_robot_state_publisher = launch_ros.actions.Node(
        package='robot_state_publisher',
        executable ='robot_state_publisher',
        parameters =[{'robot_description':robot_description_value}]
    )
    #启动节点,关节状态发布节点
    # action_joint_state_publisher = launch_ros.actions.Node(
    #     package='joint_state_publisher',
    #     executable ='joint_state_publisher',
       
    # )
    #通过IncludeLaunchDescription包含另一个launch文件
    #ros2 launch gazebo_ros gazebo.launch.py
    action_launch_gazebo = launch.actions.IncludeLaunchDescription(
        launch.launch_description_sources.PythonLaunchDescriptionSource(
            [get_package_share_directory('gazebo_ros'),'/launch', '/gazebo.launch.py']
        ),
        #传递参数
        launch_arguments=[('world',default_gazebo_world_path),('verbose','true')]
    )
    #加载模型，ros2 run gazebo_ros spawn_entity.py 参数
    action_spawn_entity = launch_ros.actions.Node(
        package='gazebo_ros',
        executable ='spawn_entity.py', 
        arguments =['-topic', '/robot_description', '-entity', robot_name_in_model]      
    )
    #加载并激活fishbot_joint_state_broadcaster， ros2 control load_controller fishbot_joint_state_broadcaster  --set-state active
    action_load_joint_state_controller = launch.actions.ExecuteProcess(
        cmd ='ros2 control load_controller fishbot_joint_state_broadcaster --set-state active'.split(' '),
        output='screen'
    )

    #加载并激活力控制器fishbot_effort_controller:， 
    action_load_effort_controller = launch.actions.ExecuteProcess(
        cmd ='ros2 control load_controller fishbot_effort_controller --set-state active'.split(' '),
        output='screen'
    )
    #加载并激活轮差速控制器fishbot_diff_drive_controller:， 
    action_load_diff_controller = launch.actions.ExecuteProcess(
        cmd ='ros2 control load_controller fishbot_diff_drive_controller --set-state active'.split(' '),
        output='screen'
    )
    
    return launch.LaunchDescription([
        action_declare_arg_mode_path,
        action_robot_state_publisher, 
        action_launch_gazebo,
        action_spawn_entity,
        #事件动作，加载机器人结束后执行，
        launch.actions.RegisterEventHandler(
            event_handler=launch.event_handlers.OnProcessExit(
                target_action=action_spawn_entity,
                on_exit=[action_load_joint_state_controller],
            )
        ),
        #事件动作，加载力或着差速控制器：action_load_joint_state_controller结束后执行，
        launch.actions.RegisterEventHandler(
            event_handler=launch.event_handlers.OnProcessExit(
                target_action=action_load_joint_state_controller,
                #on_exit=[action_load_effort_controller],#激活力控制器
                on_exit=[action_load_diff_controller],#激活差速控制器
            )
        ),

    ])